/*
 * test.cpp
 *
 *  Created on: 12-08-2011
 *      Author: tomasz
 */

////////////////////////////////////////////////////////////////
//                                                            //
// main.c++                                                   //
// beta version 0.3 - 9/9/97)                                 //
//                                                            //
// George Stetten and Korin Crawford                          //
// copyright given to the public domain                       //
//                                                            //
// Please email comments to email@stetten.com,                //
//                                                            //
////////////////////////////////////////////////////////////////


#include "glutMaster.h"
#include "glutWindow.h"
#include "GraphWindow.h"
#include "VisualReporter.h"

//-----------------------------------------------------------------------------
// RSIM includes:
//-----------------------------------------------------------------------------
#include "RSIM_INC.h"

#include <cstdlib>
#include <iomanip>
#include <fstream>

//-----------------------------------------------------------------------------
// PQP includes:
//-----------------------------------------------------------------------------
#include "../utils/include/PQP.h"
#include <cmath>
#include <cstdio>

GlutMaster * glutMaster;
graph::GraphWindow * myWindow = 0;

void pqp_test();

void setState(RSIM::State& state) {
	double q1[] = { 1, 2, 3 };
	double q2[] = { 0, 0, -PI / 6 };
	double q3[] = { 1, 2, 3 };
	double quat1[4], quat2[4], quat3[4];
	RSIM::Quaternion::calcQuaternionFromEulerAngles(q1[0], q1[1], q1[2],
			"Body123", quat1);
	RSIM::Quaternion::calcQuaternionFromEulerAngles(q2[0], q2[1], q2[2],
			"Body123", quat2);
	RSIM::Quaternion::calcQuaternionFromEulerAngles(q3[0], q3[1], q3[2],
			"Body123", quat3);

	//         state.updOneQ(PI/6,0,QIndex(0));

	state.updQ(quat1, 0); // Q for the node 0.
	//         state.updQ(quat2,1);     // Q for node 1
	state.updQ(quat3, 2); // Q for node 2

}

int main(void) {
	printf("RSIM simple viewer. \n");
	printf("Interface: \n");
	printf("==================\n");
	printf(
			"Moving mouse while left button pressed - rotations along OX and OY\n");
	printf("Moving mouse while right button pressed - rotations along OZ\n");
	printf("W - toggle wireframe mode\n");
	printf(
			"A-D-Z-X: moving one of the objects used for distance calculation.\n");

	//-------------------------------------------------------------------------
		glutMaster = new GlutMaster();

		myWindow = new graph::GraphWindow(glutMaster, 500, 500, // height, width
				200, 400, // initPosition (x,y)
				"RSIM Simple Viewer v1.0"); // title

	//----------------------------------------------------------------------

	double mass = 1.0;
	Mat3x3 I;
	I.Diagonal(1.0, 2.0, 3.0);

	MultibodySystem MBS1, MBS2, MBS3, MBS4;
	RigidBodyNode *tmp1, *tmp2, *tmp3;

	tmp1 = MBS1.setRoot(RigidBody("Pin", MassProperties(mass, I), Transform(
			Vect3(-1.5, 0, 0)), Transform(Vect3(0, 0, 0))));
	tmp2 = MBS1.addNodeTo(tmp1, RigidBody("Pin", MassProperties(mass, I),
			Transform(Vect3(-1.5, 0, 0)), Transform(Vect3(1.5, 0, 0))));
	tmp3 = MBS1.addNodeTo(tmp2, RigidBody("Pin", MassProperties(mass, I),
			Transform(Vect3(-1.5, 0, 0)), Transform(Vect3(1.5, 0, 0))));
	MBS1.setTopologyMaps();
	State state1(MBS1);

	tmp1 = MBS2.setRoot(RigidBody("Pin", MassProperties(mass, I), Transform(
			Vect3(-1.5, 0, 0)), Transform(Vect3(0, 0, 0))));
	tmp2 = MBS2.addNodeTo(tmp1, RigidBody("Pin", MassProperties(mass, I),
			Transform(Vect3(-1.5, 0, 0)), Transform(Vect3(1.5, 0, 0))));
	tmp3 = MBS2.addNodeTo(tmp2, RigidBody("Pin", MassProperties(mass, I),
			Transform(Vect3(-1.5, 0, 0)), Transform(Vect3(1.5, 0, 0))));
	MBS2.setTopologyMaps();
	State state2(MBS2);

	SystemData SD;

	SD.addSystemData(&MBS1, &state1);
	SD.addSystemData(&MBS2, &state2);
	SD.addGravityLikeAcceleration(Vect3(0.0, -9.81, 0.0));
	SD.finalize();
	MBS1.initEOM();
	MBS1.initABA();

	MBS2.initEOM();
	MBS2.initABA();

	myWindow->addMBS(&MBS1);

//	while (1)
//	glutMaster->CallGlutMainLoopEvent();


	Integrator *Integ = new Ode45;

	Integ->addReporter(new graph::VisualReporter(SD, 0.1, glutMaster, myWindow));

	Integ->stepTo(10.0, SD);
	delete Integ;

	return 0;
}
/*
 int main(void) {

 printf("RSIM simple viewer. \n");
 printf("Interface: \n");
 printf("==================\n");
 printf("Moving mouse while left button pressed - rotations along OX and OY\n");
 printf("Moving mouse while right button pressed - rotations along OZ\n");
 printf("W - toggle wireframe mode\n");
 printf("A-D-Z-X: moving one of the objects used for distance calculation.\n");

 glutMaster = new GlutMaster();

 myWindow = new graph::GraphWindow(glutMaster, 500, 500, // height, width
 200, 400, // initPosition (x,y)
 "RSIM Simple Viewer v1.0"); // title
 //-------------------------------------------------------------------------
 // Creating a Multi Body System --> excerpt from Kishor's code
 //-------------------------------------------------------------------------
 //TODO: Nic tu nie powinno poniżej być zakomentowane!
 double mass = 1.0;
 RSIM::Mat3x3 I;
 I.Diagonal(1.0, 2.0, 3.0);

 RSIM::MultibodySystem MBS1, MBS2, MBS3, MBS4;
 RSIM::RigidBodyNode *tmp1, *tmp2, *tmp3;

 tmp1 = MBS1.setRoot(RSIM::RigidBody("Ball", RSIM::MassProperties(mass, I), RSIM::Transform(
 RSIM::Vect3(-1.5, 0, 0)), RSIM::Transform(RSIM::Vect3(0, 0, 0))));
 tmp2 = MBS1.addNodeTo(tmp1, RSIM::RigidBody("Pin", RSIM::MassProperties(mass, I),
 RSIM::Transform(RSIM::Vect3(-1.5, 0, 0)), RSIM::Transform(RSIM::Vect3(1.5, 0, 0))));
 tmp3 = MBS1.addNodeTo(tmp2, RSIM::RigidBody("Ball", RSIM::MassProperties(mass, I),
 RSIM::Transform(RSIM::Vect3(-3.0, 0, 0)), RSIM::Transform(RSIM::Vect3(1.5, 0, 0))));
 MBS1.setTopologyMaps();
 RSIM::State state1(MBS1);
 setState(state1);

 tmp1 = MBS2.setRoot(RSIM::RigidBody("Ball", RSIM::MassProperties(mass, I), RSIM::Transform(
 RSIM::Vect3(-1.5, 0, 0)), RSIM::Transform(RSIM::Vect3(5, 0, 0))));
 tmp2 = MBS2.addNodeTo(tmp1, RSIM::RigidBody("Pin", RSIM::MassProperties(mass, I),
 RSIM::Transform(RSIM::Vect3(-1.5, 0, 0)), RSIM::Transform(RSIM::Vect3(1.5, 0, 0))));
 tmp3 = MBS2.addNodeTo(tmp2, RSIM::RigidBody("Ball", RSIM::MassProperties(mass, I),
 RSIM::Transform(RSIM::Vect3(-3.0, 0, 0)), RSIM::Transform(RSIM::Vect3(1.5, 0, 0))));
 MBS2.setTopologyMaps();
 RSIM::State state2(MBS2);
 setState(state2);

 RSIM::SystemData SD;

 SD.addSystemData(&MBS1, &state1);
 SD.addSystemData(&MBS2, &state2);

 SD.constrain("Ball", RSIM::Transform(RSIM::Vect3(0, 0, 0)), RSIM::SystemIndex(0),
 RSIM::NodeIndex(2), RSIM::Transform(RSIM::Vect3(0, 0, 0)), RSIM::SystemIndex(1), RSIM::NodeIndex(
 2));

 SD.constrainToGround("Ball", RSIM::Transform(RSIM::Vect3(0, 0, 0)), RSIM::SystemIndex(0),
 RSIM::NodeIndex(2), RSIM::Transform(RSIM::Vect3(3.0, 0, 0)));

 SD.finalize();

 SD.updSystemPosition();
 SD.attemptSatisfyPositionLevelConstraints();

 Integrator *Ineg = new Ode45;
 Ineg->stepTo(10, MBS1, state1);
 graph::VisualReporter rep(SD, 1000);
 Ineg->addReporter(&rep);

 //-------------------------------------------------------------------------

 //myWindow->setMBS(&MBS1);
 myWindow->addMBS(&MBS1);

 //pqp_test();
 glutMaster->CallGlutMainLoop();

 // testing PQP library...


 return 0;
 }
 */

//#define PI 3.14159265359
#define LISTS 0

void pqp_test() {

	// initialize PQP model pointers

	PQP_Model *b1 = new PQP_Model;
	PQP_Model *b2 = new PQP_Model;

	// Add trianges to form tori

	fprintf(stderr, "loading tris into PQP_Model objects...");
	fflush( stderr);

	PQP_REAL a = (PQP_REAL) 1.0; // major radius of the tori
	PQP_REAL b = (PQP_REAL) 0.2; // minor radius of the tori

	int n1 = 50; // tori will have n1*n2*2 triangles each
	int n2 = 50;

	int uc, vc;
	int count = 0;

	b1->BeginModel();
	b2->BeginModel();
	for (uc = 0; uc < n1; uc++) {
		for (vc = 0; vc < n2; vc++) {
			PQP_REAL u1 = (PQP_REAL)(2.0 * PI * uc) / n1;
			PQP_REAL u2 = (PQP_REAL)(2.0 * PI * (uc + 1)) / n1;
			PQP_REAL v1 = (PQP_REAL)(2.0 * PI * vc) / n2;
			PQP_REAL v2 = (PQP_REAL)(2.0 * PI * (vc + 1)) / n2;

			PQP_REAL p1[3], p2[3], p3[3], p4[3];

			p1[0] = (a - b * cos(v1)) * cos(u1);
			p2[0] = (a - b * cos(v1)) * cos(u2);
			p3[0] = (a - b * cos(v2)) * cos(u1);
			p4[0] = (a - b * cos(v2)) * cos(u2);
			p1[1] = (a - b * cos(v1)) * sin(u1);
			p2[1] = (a - b * cos(v1)) * sin(u2);
			p3[1] = (a - b * cos(v2)) * sin(u1);
			p4[1] = (a - b * cos(v2)) * sin(u2);
			p1[2] = b * sin(v1);
			p2[2] = b * sin(v1);
			p3[2] = b * sin(v2);
			p4[2] = b * sin(v2);

			b1->AddTri(p1, p2, p3, count);
			b1->AddTri(p4, p2, p3, count + 1);
			b2->AddTri(p1, p2, p3, count);
			b2->AddTri(p4, p2, p3, count + 1);

			count += 2;
		}
	}

	fprintf(stderr, "done\n");
	fflush(stderr);
	fprintf(stderr, "Tori have %d triangles each.\n", count);
	fprintf(stderr, "building hierarchies...");
	fflush(stderr);
	b1->EndModel();
	b2->EndModel();
	fprintf(stderr, "done.\n");
	b1->MemUsage(1);
	b2->MemUsage(1);
	fflush(stderr);

	// now we are free to call the proximity routines.
	// but first, construct the transformations that define the placement
	// of our two hierarchies in world space:

	// this placement causes them to overlap a large amount.

	PQP_REAL R1[3][3], R2[3][3], T1[3], T2[3];

	R1[0][0] = R1[1][1] = R1[2][2] = 1.0;
	R1[0][1] = R1[1][0] = R1[2][0] = 0.0;
	R1[0][2] = R1[1][2] = R1[2][1] = 0.0;

	R2[0][0] = R2[1][1] = R2[2][2] = 1.0;
	R2[0][1] = R2[1][0] = R2[2][0] = 0.0;
	R2[0][2] = R2[1][2] = R2[2][1] = 0.0;

	T1[0] = 1.0;
	T1[1] = 0.0;
	T1[2] = 0.0;
	T2[0] = 0.0;
	T2[1] = 0.0;
	T2[2] = 0.0;

	// perform a collision query

	PQP_CollideResult cres;
	PQP_Collide(&cres, R1, T1, b1, R2, T2, b2, PQP_ALL_CONTACTS);

	// looking at the report, we can see where all the contacts were, and
	// also how many tests were necessary:

	printf("\nAll contact collision query between overlapping tori:\n");
	printf("Num BV tests: %d\n", cres.NumBVTests());
	printf("Num Tri tests: %d\n", cres.NumTriTests());
	printf("Num contact pairs: %d\n", cres.NumPairs());
#if LISTS
	int i;
	for(i=0; i<cres.NumPairs(); i++)
	{
		printf("\t contact %4d: tri %4d and tri %4d\n",
				i,
				cres.Id1(i),
				cres.Id2(i));
	}
#endif

	// Notice the PQP_ALL_CONTACTS flag we used in the call to PQP_Collide.
	// The alternative is to use the PQP_FIRST_CONTACT flag, instead.
	// The result is that the collide routine searches for any contact,
	// but not all of them.  It can take many many fewer tests to locate a single
	// contact.

	PQP_Collide(&cres, R1, T1, b1, R2, T2, b2, PQP_FIRST_CONTACT);

	printf("\nFirst contact collision query between overlapping tori:\n");
	printf("Num BV tests: %d\n", cres.NumBVTests());
	printf("Num Tri tests: %d\n", cres.NumTriTests());
	printf("Num contact pairs: %d\n", cres.NumPairs());
#if LISTS
	for(i=0; i<cres.NumPairs(); i++)
	{
		printf("\t contact %4d: tri %4d and tri %4d\n",
				i,
				cres.Id1(i),
				cres.Id2(i));
	}
#endif

	// Perform a distance query, which should return a distance of 0.0

	PQP_DistanceResult dres;
	PQP_Distance(&dres, R1, T1, b1, R2, T2, b2, 0.0, 0.0);

	printf("\nDistance query between overlapping tori\n");
	printf("Num BV tests: %d\n", dres.NumBVTests());
	printf("Num Tri tests: %d\n", dres.NumTriTests());
	printf("Distance: %f\n", dres.Distance());

	// by rotating one of them around the x-axis 90 degrees, they
	// are now interlocked, but not quite touching.

	R1[0][0] = 1.0;
	R1[0][1] = 0.0;
	R1[0][2] = 0.0;
	R1[1][0] = 0.0;
	R1[1][1] = 0.0;
	R1[1][2] = -1.0;
	R1[2][0] = 0.0;
	R1[2][1] = 1.0;
	R1[2][2] = 0.0;

	PQP_Collide(&cres, R1, T1, b1, R2, T2, b2, PQP_FIRST_CONTACT);

	printf("\nCollision query between interlocked but nontouching tori:\n");
	printf("Num BV tests: %d\n", cres.NumBVTests());
	printf("Num Tri tests: %d\n", cres.NumTriTests());
	printf("Num contact pairs: %d\n", cres.NumPairs());
#if LISTS
	for(i=0; i<cres.NumPairs(); i++)
	{
		printf("\t contact %4d: tri %4d and tri %4d\n",
				i,
				cres.Id1(i),
				cres.Id2(i));
	}
#endif

	// Perform a distance query - the distance found should be greater than zero

	PQP_Distance(&dres, R1, T1, b1, R2, T2, b2, 0.0, 0.0);

	printf("\nDistance query between interlocked but nontouching tori\n");
	printf("Num BV tests: %d\n", dres.NumBVTests());
	printf("Num Tri tests: %d\n", dres.NumTriTests());
	printf("Distance: %f\n", dres.Distance());

	// Perform two tolerance queries. One tolerance setting is greater than the
	// distance between the models, and one tolerance is less than the distance.

	PQP_ToleranceResult tres;
	PQP_REAL tolerance = (PQP_REAL) .60;
	PQP_Tolerance(&tres, R1, T1, b1, R2, T2, b2, tolerance);

	printf("\nTolerance query between interlocked but nontouching tori\n"
		"with tolerance %f\n", tolerance);
	printf("Num BV tests: %d\n", tres.NumBVTests());
	printf("Num Tri tests: %d\n", tres.NumTriTests());
	printf("Closer than tolerance? %f", tolerance);
	if (tres.CloserThanTolerance())
		printf("yes.\n");
	else
		printf("no.\n");

	tolerance = (PQP_REAL) .40;
	PQP_Tolerance(&tres, R1, T1, b1, R2, T2, b2, tolerance);

	printf("\nTolerance query between interlocked but nontouching tori\n"
		"with tolerance %f\n", tolerance);
	printf("Num BV tests: %d\n", tres.NumBVTests());
	printf("Num Tri tests: %d\n", tres.NumTriTests());
	printf("Closer than tolerance? %f ", tolerance);
	if (tres.CloserThanTolerance())
		printf("yes.\n");
	else
		printf("no.\n");

	// by moving one of the tori closer to the other, they
	// almost touch.  This is the case that requires a lot
	// of work wiht methods which use bounding boxes of limited
	// aspect ratio.  Oriented bounding boxes are more efficient
	// at determining noncontact than spheres, octree, or axis-aligned
	// bounding boxes for scenarios like this.  In this case, the interlocked
	// tori are separated by 0.0001 at their closest point.


	T1[0] = (PQP_REAL) 1.5999;

	PQP_Collide(&cres, R1, T1, b1, R2, T2, b2, PQP_FIRST_CONTACT);

	printf("\nCollision query on interlocked and almost touching tori:\n");
	printf("Num BV tests: %d\n", cres.NumBVTests());
	printf("Num Tri tests: %d\n", cres.NumTriTests());
	printf("Num contact pairs: %d\n", cres.NumPairs());
#if LISTS
	for(i=0; i<cres.NumPairs(); i++)
	{
		printf("\t contact %4d: tri %4d and tri %4d\n",
				i,
				cres.Id1(i),
				cres.Id2(i));
	}
#endif

	PQP_Distance(&dres, R1, T1, b1, R2, T2, b2, 0.0, 0.0);

	printf("\nDistance query between interlocked and almost touching tori\n");
	printf("Num BV tests: %d\n", dres.NumBVTests());
	printf("Num Tri tests: %d\n", dres.NumTriTests());
	printf("Distance: %f\n", dres.Distance());

	tolerance = (PQP_REAL) 0.00015;
	PQP_Tolerance(&tres, R1, T1, b1, R2, T2, b2, tolerance);

	printf("\nTolerance query between interlocked and almost touching tori\n"
		"with tolerance %f\n", tolerance);
	printf("Num BV tests: %d\n", tres.NumBVTests());
	printf("Num Tri tests: %d\n", tres.NumTriTests());
	printf("Closer than tolerance? %f", tolerance);
	if (tres.CloserThanTolerance())
		printf("yes.\n");
	else
		printf("no.\n");

	tolerance = (PQP_REAL) 0.00005;
	PQP_Tolerance(&tres, R1, T1, b1, R2, T2, b2, tolerance);

	printf("\nTolerance query between interlocked and almost touching tori\n"
		"with tolerance %f\n", tolerance);
	printf("Num BV tests: %d\n", tres.NumBVTests());
	printf("Num Tri tests: %d\n", tres.NumTriTests());
	printf("Closer than tolerance? %f", tolerance);
	if (tres.CloserThanTolerance())
		printf("yes.\n");
	else
		printf("no.\n");

	delete b1;
	delete b2;

}

